#include <stdio.h>
#include <string.h>

#include "packet.h"

const unsigned char PSTART = 0x10;

unsigned char calc_cs(const unsigned char *_buffer, size_t _len)
{
	unsigned char c = 0;

	size_t i = 0;

	for(i = 0; i < _len; i++)
	{
		c += _buffer[i];
	}

	return 0 - c;
}

int build_packet(unsigned char *_dst, const unsigned char *_src, size_t _len, size_t _buff_len)
{
	if (_len + 4 > _buff_len)
	{
		return -1;
	}

	_dst[0] = 0x10;
	_dst[1] = _len;

	memcpy(_dst + 2, _src, _len);

	_dst[_len + 2] = 0x10;
	_dst[_len + 2 + 1] = calc_cs(_dst + 1, _len + 1);

	return 0;
}

int parse_packet(const unsigned char *_src, unsigned int _src_len,  const unsigned char **_dst, unsigned int *_dst_len)
{
    unsigned int pos = 0;

    if (_src[pos] != PSTART)
        return -1;

    pos++;

    unsigned int len = _src[pos];     // data length

    if ((len + 2) > _src_len)
    	return -1;

    pos += len + 1;

    if (_src[pos] != PSTART)
        return -1;

    pos++;

    if (_src[pos] != calc_cs(_src + 1, len + 1))
        return -1;

    *_dst = (_src + 2);
    *_dst_len = len;

    return 0;
}

void dump_packet(const unsigned char *_src, unsigned int _src_len)
{
    size_t i;
    for(i = 0; i < _src_len; i++)
    {
	  if (i > 0)
	  {
		printf(" ");
	  }

	  printf("%02x", (int) (_src[i]));
    }
}
